#include <fstream>
#include <cmath>
#include "angle.h"
#include "earth.h"
#include "rotation.h"
#include "../include/1mathMyDefine.h"
#include "../include/1earthMyDefine.h"
#include "../include/class.h"
#include "../include/inertial_navigation.h"

using namespace std;
using namespace Eigen;

///////////////////////preparations

State inertial_navigation(State stateBeforeBefore,
                          State stateBefore,
                          Quaterniond q_bBefore2nBefore,
                          IMUDATA imuDataBefore,
                          IMUDATA imuDataNow) {
    ///////////////////////////////前一时刻和前两个时刻的信息
    /*前两时刻的状态 stateBeforeBefore*/
    /*前一时刻   stateBefore*/
    /*前一时刻的四元数 q_bBefore2nBefore*/

    /*现在时刻的状态*/
    State stateNow;
    /*get 时间间隔*/
    double T = imuDataNow.getTime() - imuDataBefore.getTime();
    double T1 = imuDataBefore.getTime() - stateBeforeBefore.getTime();
    //////////////////////////////////////////////get vel
    Vector3d vNow = getVelocity(
            stateBeforeBefore,
            stateBefore,
            q_bBefore2nBefore,
            imuDataBefore,
              imuDataNow,
            T,
            T1);

    /////////////////////////////////////////////get pos
    Vector3d positionNow = getPosition(stateBefore, vNow, T);

    /////////////////////////////////////////////get att
    Vector3d positionMiddle = (positionNow + stateBefore.get_pos()) / 2;
    Vector3d velocityMiddle = (vNow + stateBefore.get_vel()) / 2;

    /*get att quaternion*/
    Quaterniond q_bNow2nNow = getAttitudeQuaternion(
            q_bBefore2nBefore,
            imuDataBefore,
            imuDataNow,
            positionMiddle,
            velocityMiddle,
            T);

    ///////////////////////后续数据更新处理
    stateNow.changeState(q_bNow2nNow,
                         vNow,
                         positionNow,
                         imuDataNow.getTime());

    return stateNow;
}



//////////////////////////////////////////get velocity
/*只有Vn外推过，其他的都用前一时刻*/
Vector3d getVelocity(
        State stateBeforeBefore,
        State stateBefore,
        Quaterniond q_bBefore2nBefore,
        IMUDATA imuDataBefore,
        IMUDATA imuDataNow,
        double T,
        double T1){

    /*前两个时刻速度*/
    Vector3d vBeforeBefore = stateBeforeBefore.get_vel();
    /*前一时刻速度*/
    Vector3d vBefore = stateBefore.get_vel();


    ///////////////////////////////////get重力积分项

    /*得到中间时刻vbTon*/
    //Vector3d vMiddle = vBefore;
    Vector3d vMiddle = vBefore + (vBefore - vBeforeBefore) * T / T1 / 2.0;
//    Vector3d vMiddle = 3.0 / 2.0 * vBefore - 1.0 / 2.0 * vBeforeBefore;

    //地球自自转角速度在n系投影
    /*前一时刻的纬度代替现在的纬度*/
    double latitude = stateBefore.getLatitude();
    Vector3d w_ie2n = Earth::iewn(latitude);

    //牵连角速度在n系投影
    /*get rm rn*/
    Vector2d rmn  = Earth::meridianPrimeVerticalRadius(latitude);
    /*用前一时刻的位置代替现在时刻的位置,
    * 用前一时刻的速度代替现在的速度*/
    Vector3d positionBefore = stateBefore.get_pos();
    vBefore = stateBefore.get_vel();
    Vector3d w_en2n = Earth::enwn(rmn, positionBefore, vBefore);

    /*get重力*/
    double gl = Earth::gravity(positionBefore);
    Vector3d glTon(0, 0, gl);

    /*get 重力积分项*/
    Vector3d delta_gAndCorAndK2n = (glTon - (2 * w_ie2n + w_en2n).cross(vMiddle)) * T;


    ///////////////////////////////////////////////////get 比力积分项目
    /* get delta_fk2bBefore*/
    Vector3d zeta = (w_ie2n + w_en2n) * T;
    Matrix3d zetaMatrix;
    zetaMatrix << 0, -1 * zeta[2],      zeta[1],
            zeta[2],            0, -1 * zeta[0],
            -1 * zeta[1],      zeta[0],            0;
    Vector3d deltaVNow = imuDataNow.getAccelData();
    Vector3d deltaVBefore = imuDataBefore.getAccelData();
    Vector3d deltaSetaNow = imuDataNow.getGyroData();
    Vector3d deltaSetaBefore = imuDataBefore.getGyroData();
    Vector3d delta_fk2bBefore = deltaVNow +
                                1.0 / 2.0 * deltaSetaNow.cross(deltaVNow) +
                                1.0 / 12.0 * (deltaSetaBefore.cross(deltaVNow) + deltaVBefore.cross(deltaSetaNow));

    /*get 前一时刻的方向余弦矩阵*/
    Matrix3d directionCosineMatrix_bBefore2nBefore = Rotation::quaternion2matrix(q_bBefore2nBefore);

    /*求解比比力积分项*/
    Vector3d delta_fk2n = (I_3 - 0.5 * zetaMatrix) * directionCosineMatrix_bBefore2nBefore * delta_fk2bBefore;

    ///////////////////////////////////////get 最终速度
    Vector3d vNow = vBefore + delta_fk2n + delta_gAndCorAndK2n;
    return vNow;
}


////////////////////////////////////////get att_quat
Quaterniond get_b_matrix(Vector3d faiK){//Quaternion里面存储似乎是实部在后面，虚部在前面
    Vector4d vet;
    if (faiK.norm() == 0) //防止数据出现0，0，0的情况
    {
        vet << (0.5 * faiK), cos((0.5 * faiK).norm());
    }
    else {
        vet << sin((0.5 * faiK).norm()) * (0.5 * faiK) / ((0.5 * faiK).norm()),
                cos((0.5 * faiK).norm());
    }

    return Quaterniond(vet);
}

Quaterniond get_n_matrix(Vector3d zetaK){
    Vector4d vet;
    Quaterniond m;
    vet << -1 * sin((0.5 * zetaK).norm()) / ((0.5 * zetaK).norm()) * (0.5 * zetaK),
            cos((0.5 * zetaK).norm());

    return Quaterniond(vet);
}

Quaterniond getAttitudeQuaternion(Quaterniond q_bBefore2nBefore,
                                  IMUDATA imuDataBefore,
                                  IMUDATA imuDataNow,
                                  Vector3d positionMiddle,
                                  Vector3d velocityMiddle,
                                  double T){



    Vector3d w_ie2n;//地球自转角速度投影到n系
    Vector3d w_en2n;//牵连角速度投影到n系
    Quaterniond q_nBefore2nNow;//n系变化的四元数
    Quaterniond q_bNow2nNow;//n系当前时刻的四元数


    /////////得到b系变化的四元数
    Vector3d thetNow = imuDataNow.getGyroData();//当前时刻陀螺仪的输出量
    Vector3d thetBefore = imuDataBefore.getGyroData();//前一时刻陀螺仪的输出量
    /*get等效旋转矢量*/
    Vector3d faiK = thetNow + 1.0 / 12.0 * thetBefore.cross(thetNow);
    /*get b系变化的四元数*/
    Quaterniond q_bNow2bBefore = get_b_matrix(faiK);//b系变化的四元数

    //////////get n系变化的四元数
    /*使用外推的位置和速度*/
    double latitude = positionMiddle[0];

    /*地球自转角速度投影到n系*/
    w_ie2n = Earth::iewn(latitude);
    /* get Rm Rn*/
    Vector2d rmn;
    rmn  = Earth::meridianPrimeVerticalRadius(latitude);
    /*get 牵连角速度在n系下的投影*/
    w_en2n = Earth::enwn(rmn, positionMiddle, velocityMiddle);
    /*get n系变化的等效旋转矢量*/
    Vector3d  zetaK = (w_ie2n + w_en2n) * T;
    /*get n系变化的四元数*/
    q_nBefore2nNow = get_n_matrix(zetaK);

    ////////////////当前时刻的b系投影在n系的四元数
    q_bNow2nNow = q_nBefore2nNow * q_bBefore2nBefore * q_bNow2bBefore;

    /*四元数的归一化*/
    return q_bNow2nNow.normalized();
}






//////////////////////////////////////////get position
Vector3d getPosition(State stateBefore, Vector3d vNow, double T){
    double vNorthNow = vNow[0];
    double vEastNow = vNow[1];
    double vDownNow = vNow[2];
    double vNorthBefore = stateBefore.getVNorth();
    double vEastBefore = stateBefore.getVEast();
    double vDownBefore = stateBefore.getVDown();
    double latitudeBefore = stateBefore.getLatitude();
    double longitudeBefore = stateBefore.getLongitude();
    double heightBefore = stateBefore.getHeight();

    /////////////////////////get height
    double heightNow;
    heightNow = heightBefore - 1.0 / 2.0 * (vDownBefore + vDownNow) * T;
    /*get平均高度*/
    double hAverage = (heightNow + heightBefore) / 2;

    ////////////////////////get latitude纬度
    double latitudeNow;
    double RMBefore = a * (1 - e2) / sqrt(pow(1 - e2 * sin(latitudeBefore) * sin(latitudeBefore), 3) );
    latitudeNow = latitudeBefore + (vNorthNow + vNorthBefore) / (2.0 * RMBefore + 2.0 * hAverage) * T;
    /*get 平均纬度*/
    double latitudeAverage = (latitudeBefore + latitudeNow) / 2;
    /*get 平均RN*/
    double RNAverage = a / sqrt(1 - e2 * sin(latitudeAverage) * sin(latitudeAverage));

    ////////////////////////get longitude经度
    double longitudeNow;
    longitudeNow = longitudeBefore +
                   (vEastBefore + vEastNow) / (2 * (RNAverage + hAverage) * cos(latitudeAverage)) * T;

    //////////////////////get 现在时刻的位置
    Vector3d positionNow;
    positionNow << latitudeNow, longitudeNow, heightNow;
    return positionNow;
}





////////////////////////////////////////不进行外推的惯性导航
//State inertial_navigation(State stateBefore,
//                                        Quaterniond q_bBefore2nBefore,
//                                        IMUDATA imuDataBefore,
//                                        IMUDATA imuDataNow) {
//    ///////////////////////////////前一时刻和前两个时刻的信息
//    /*前两时刻的状态 stateBeforeBefore*/
//    /*前一时刻   stateBefore*/
//    /*前一时刻的四元数 q_bBefore2nBefore*/
//
//    /*现在时刻的状态*/
//    State stateNow;
//    /*get 时间间隔*/
//    double T = imuDataNow.getTime() - imuDataBefore.getTime();
//
//    //////////////////////////////////////////////get vel
//    Vector3d vNow = getVelocity(
//            stateBefore,
//            q_bBefore2nBefore,
//            imuDataBefore,
//            imuDataNow,
//            T);
//
//    /////////////////////////////////////////////get pos
//    Vector3d positionNow = getPosition(stateBefore, vNow, T);
//
//    /////////////////////////////////////////////get att
//    Vector3d positionMiddle = (positionNow + stateBefore.get_pos()) / 2;
//    Vector3d velocityMiddle = (vNow + stateBefore.get_vel()) / 2;
//
//    /*get att quaternion*/
//    Quaterniond q_bNow2nNow = getAttitudeQuaternion(
//            q_bBefore2nBefore,
//            imuDataBefore,
//            imuDataNow,
//            positionMiddle,
//            velocityMiddle,
//            T);
//
//    ///////////////////////后续数据更新处理
//    stateNow.changeState(q_bNow2nNow,
//                         vNow,
//                         positionNow,
//                         imuDataNow.getTime());
//
//    return stateNow;
//}
//
//
//
//
//
////////////////////////////////////////////////////速度不进行外推
///*不外推*/
//Vector3d getVelocity(
//        State stateBefore,
//        Quaterniond q_bBefore2nBefore,
//        IMUDATA imuDataBefore,
//        IMUDATA imuDataNow,
//        double T){
//
//    /*前一时刻速度*/
//    Vector3d vBefore = stateBefore.get_vel();
//
//
//    ///////////////////////////////////get重力积分项
//    /*得到中间时刻vbTon*/
//    Vector3d vMiddle = vBefore;
//
//    //地球自自转角速度在n系投影
//    /*前一时刻的纬度代替现在的纬度*/
//    double latitude = stateBefore.getLatitude();
//    Vector3d w_ie2n = Earth::iewn(latitude);
//
//    //牵连角速度在n系投影
//    /*get rm rn*/
//    Vector2d rmn  = Earth::meridianPrimeVerticalRadius(latitude);
//    /*用前一时刻的位置代替现在时刻的位置,
//    * 用前一时刻的速度代替现在的速度*/
//    Vector3d positionBefore = stateBefore.get_pos();
//    vBefore = stateBefore.get_vel();
//    Vector3d w_en2n = Earth::enwn(rmn, positionBefore, vBefore);
//
//
//
//
//    /*get重力*/
//    double gl = Earth::gravity(positionBefore);
//    Vector3d glTon(0, 0, gl);
//
//    /*get 重力积分项*/
//    Vector3d delta_gAndCorAndK2n = (glTon - (2 * w_ie2n + w_en2n).cross(vMiddle)) * T;
//
//
//
//    ///////////////////////////////////////////////////get 比力积分项目
//    /* get delta_fk2bBefore*/
//    Vector3d zeta = (w_ie2n + w_en2n) * T;
//    Matrix3d zetaMatrix;
//    zetaMatrix <<                 0, -1 * zeta[2],      zeta[1],
//            zeta[2],            0, -1 * zeta[0],
//            -1 * zeta[1],      zeta[0],            0;
//    Vector3d deltaVNow = imuDataNow.getAccelData();
//    Vector3d deltaVBefore = imuDataBefore.getAccelData();
//    Vector3d deltaSetaNow = imuDataNow.getGyroData();
//    Vector3d deltaSetaBefore = imuDataBefore.getGyroData();
//    Vector3d delta_fk2bBefore = deltaVNow +
//                                1.0 / 2.0 * deltaSetaNow.cross(deltaVNow) +
//                                1.0 / 12.0 * (deltaSetaBefore.cross(deltaVNow) + deltaVBefore.cross(deltaSetaNow));
//
//    /*get 前一时刻的方向余弦矩阵*/
//    Matrix3d directionCosineMatrix_bBefore2nBefore = Rotation::quaternion2matrix(q_bBefore2nBefore);
//
//    /*求解比比力积分项*/
//    Vector3d delta_fk2n = (I_3 - 0.5 * zetaMatrix) * directionCosineMatrix_bBefore2nBefore * delta_fk2bBefore;
//
//    ///////////////////////////////////////get 最终速度
//    Vector3d vNow = vBefore + delta_fk2n + delta_gAndCorAndK2n;
//    return vNow;
//}






